#!/usr/bin/python3

import time
from threading import Thread
import serial


recv_buff = []
my_count = 0

s = serial.Serial('/dev/ttyS0', 9600, timeout = 0.1)
queryCmd = [0xEB, 0x90, 0xEB, 0x90, 0xEB, 0x90, 0x01, 0xA0, 0x00, 0x6F, 0x52, 0x7F]


def crc(data, crc_len):
	"""Calculates the Tracer CRC for the given data"""
	i = j = r1 = r2 = r3 = r4 = 0
	result = 0

	r1 = data[0]
	r2 = data[1]
	crc_buff = 2

	for i in range(0, crc_len - 2):
		r3 = data[crc_buff]
		crc_buff += 1

		for j in range(0, 8):
			r4 = r1
			r1 = (r1 * 2) & 0xFF

			if r2 & 0x80:
				r1 += 1
			r2 = (r2 * 2) & 0xFF

			if r3 & 0x80:
				r2 += 1
			r3 = (r3 * 2) & 0xFF

			if r4 & 0x80:
				r1 ^= 0x10
				r2 ^= 0x41

	result = (r1 << 8) | r2

	return result


def RequestThread():
	while (True):
		global recv_buff
		global my_count
		recv_buff=[]
		print("Send request")
		print("count = ", my_count)
		my_count += 1
		s.write(queryCmd)
		t0 = time.perf_counter()
		data = s.read(36)
		t = time.perf_counter() - t0
		print(type(data))
		print(' '.join(format(c, '02X') for c in data))
		print("receive length: ", len(data))
		print("time = ", t)
		print("\n")

		data_crc = crc(data[6:6+27] + b'\0\0', 29)
		data_crc_a = data[33] * 256 + data[34]

		if (data_crc == data_crc_a):
			print("CRC ok")

		time.sleep(1)

t = Thread(target = RequestThread)
t.start()

while(True):
	time.sleep(1)


